/*
 * @Author: your name
 * @Date: 2022-04-19 16:16:12
 * @LastEditTime: 2022-07-02 15:26:10
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: \cruise_cpp\cruise.cpp
 */

#include<iostream>
#include <vector>
#include <string>
#include<fstream>
#include<typeinfo>
#include<ctime>
#include<algorithm>
#include <io.h>
#include <string.h>


using namespace std ;

// 宏定义区
#define COL 160
#define ROW 120
#define MIDDLE int(COL/2)
#define BLACK 0
#define WHITE 255
#define MAT_FROM_TXT
#define LEFT_DIR 0
#define RIGHT_DIR 1
#define Y_MIN 39
#define Y_MAX  ROW-1
#define HALF_ROAD_WIDTH 80

// typedef 区
typedef int16_t int16;
typedef unsigned char uchar;

// 电脑上使用int16，板子上使用uchar
#define IMG_TYPE int16
#define DEBUG 1


// 主要的数据结构，有左边和右边
struct Direction{
    int16 boundary[ROW];                        // 赛道边界，X坐标
    vector<int16>valid_row;                     // 待筛选的有效行，Y坐标
    vector<int16>loss;                          // 丢线行的vector，Y坐标
    int16 finallosey;                           // 最终丢线行，Y坐标
    vector<int16>possible_up_cross_point;       // 可能的上拐点
    vector<int16>possible_down_cross_point;     // 可能的下拐点
};

// 描述一个点的结构体
struct Point{
    int16 x;
    int16 y;
};

//全局变量区
IMG_TYPE img[ROW][COL] ={0};                       // 存储图片的二维数组
int16 midLine[ROW]={0};                         // 中线数组
int16 validRow = 0;                             // 全局的有效行的Y坐标，由左右有效行计算得出
bool cross_flag = false;                        // 十字标志
bool turn_flag = false;                         // 弯道标志
bool V_turn_flag = false;                       // V型弯标志位
int16 abnormal_line = 0;                        // 异常的行，即左边界大于右边界的那一行，为了识别V型弯

// 绝对值函数
int16 myabs(int16 a, int16 b){
    int16 c = a-b;
    if(c>=0)return c;
    else return -c;
}

void getMat(uchar* matrix){
    for(int i = 0;  i < ROW; i++){
        for(int j = 0; j <COL; j++){
            img[i][j] = matrix[i*COL + j];
        }
    }
}

/** 
 * @description: 从文本文件中创建矩阵，在window电脑上调试时需要使用由图像转化的txt文件来获得二值化图像矩阵
 * @param {int16} img  图像二维数组
 * @param {string} fileName  文件名
 * @return {*}
 */
void creatMat(IMG_TYPE img[ROW][COL], string fileName){
    ifstream infile;//定义读取文件流，相对于程序来说是in
	infile.open(fileName);//打开文件
    for (int i = 0; i < ROW; i++){//定义行循环
		for (int j = 0; j < COL; j++){//定义列循环
			infile >> img[i][j];//读取一个值（空格、制表符、换行隔开）就写入到矩阵中，行列不断循环进行
		}
	}
	infile.close();//读取完成之后关闭文件
}


/** 
 * @description: 从中间往左边找到的传统寻找跳变点方法，
 *               在一幅图像的最底行会使用到和上一行丢线时会使用
 *              （即第一次是使用传统的方法来获得左右边界的）
 * @param {Direction*} lleft  左边的结构体指针
 * @param {int16} tmp_y       当前的y坐标（当前行）
 * @param {int16} start_x     开始向左遍历的x坐标
 * @return {*}
 */
int16 leftFindChange(Direction* lleft, int16 tmp_y,int16 start_x = MIDDLE){
    bool left_have_found = 0;
    for(int x = start_x; x > 0; x-=3){
        if(img[tmp_y][x] == 255){
            if(img[tmp_y][x+1] == 0){
                lleft->boundary[tmp_y] = x;
                left_have_found = 1;
                break;
            }
            else if(img[tmp_y][x+2] == 0){
                lleft->boundary[tmp_y] = x+1;
                left_have_found = 1;
                break;
            }
            else if(img[tmp_y][x+3] == 0){
                lleft->boundary[tmp_y] = x+2;
                left_have_found = 1;
                break;
            }
            else{
                if(tmp_y < 100)
                    lleft->valid_row.push_back(tmp_y);  //防止地标出现在100-119行间，导致进入有效行数组
            }
        }
    }
    if(!left_have_found){
        lleft->boundary[tmp_y] = 0;
        lleft->loss.push_back(tmp_y);
    }
    return lleft->boundary[tmp_y];
}

/** 
 * @description: 从中间往右边找到的传统寻找跳变点方法
 *               在一幅图像的最底行会使用到和上一行丢线时会使用
 *              （即第一次是使用传统的方法来获得左右边界的）
 * @param {Direction*} rright  右边的结构体指针
 * @param {int16} tmp_y       当前的y坐标（当前行）
 * @param {int16} start_x     开始向右遍历的x坐标
 * @return {*}
 */
int16 rightFindChange(Direction* rright, int16 tmp_y,int16 start_x = MIDDLE){
    bool right_have_found = 0;
    for(int x = start_x; x < COL; x+=3){
        if(img[tmp_y][x] == 255){
            if(img[tmp_y][x-1] == 0){
                rright->boundary[tmp_y] = x;
                right_have_found = 1;
                break;
            }
            else if(img[tmp_y][x-2] == 0){
                rright->boundary[tmp_y] = x-1;
                right_have_found = 1;
                break;
            }
            else if(img[tmp_y][x-3] == 0){
                rright->boundary[tmp_y] = x-2;
                right_have_found = 1;
                break;
            }
            else{
                if(tmp_y < 100)
                    rright->valid_row.push_back(tmp_y);  //防止地标出现在100-119行间，导致进入有效行数组
            }
        }
    }
    if(!right_have_found){
        rright->boundary[tmp_y] = COL -1;
        rright->loss.push_back(tmp_y);
    }
    return rright->boundary[tmp_y];
}

/**
 * @description: 八领域方法寻找左边边界跳变点
 * @param {Direction*} lleft left结构体
 * @param {int16} tmp_y 当前的y
 * @param {int16} last_x 上一个左边界
 * @param {int16} last_mid 上一个中点
 * @return {*}
 */
int16 filedLeftFindChange(Direction* lleft,int16 tmp_y,int16 last_x,int16 last_mid=MIDDLE){
    bool left_have_found = 0;
    // 如果上一个左边界是0，即丢线，本行就使用传统寻找跳变点，反之使用八领域寻跳变点
    if(last_x != 0){
        if(img[tmp_y][last_x] == 255){
            for (int x = last_x + 1; x < COL; x++){
               if((img[tmp_y][x] == 0) &&  (img[tmp_y][x-1] == 255)){
                   lleft->boundary[tmp_y] = x-1;
                   left_have_found = 1;
                   break;
               }
            }
        }
        else{
            for (int x = last_x - 1; x > -1; x--){
               if((img[tmp_y][x] == 255) &&  (img[tmp_y][x+1] == 0)){
                   lleft->boundary[tmp_y] = x;
                   left_have_found = 1;
                   break;
               }
            }
        }
    }
    else{// 传统寻找跳变点
        leftFindChange(lleft,tmp_y,last_mid);
        left_have_found = 1;
    }
    if(!left_have_found){// 如果都没找到跳变点，视为丢线
        lleft->boundary[tmp_y] = 0;
        lleft->loss.push_back(tmp_y);
    }
    return lleft->boundary[tmp_y];
}

/**
 * @description: 八领域寻找右边边界跳变点
 * @param {Direction*} rright 右边的数据结构
 * @param {int16} tmp_y 当前的Y
 * @param {int16} last_x 上一个右边边界点
 * @param {int16} last_mid 上一个中点
 * @return {*}
 */
int16  filedRightFindChange(Direction* rright,int16 tmp_y,int16 last_x,int16 last_mid=MIDDLE){
    bool right_have_found = 0;
    // 若上一行的右边界为159，则采用传统方式寻找跳变点，反之使用八领域寻找跳变点
    if (last_x != COL-1){
        if(img[tmp_y][last_x] == 255){
            for(int x = last_x-1; x >-1;x--){
                if((img[tmp_y][x] == 0)&&(img[tmp_y][x+1] == 255)){
                    rright->boundary[tmp_y] = x+1;
                    right_have_found = 1;
                    break;
                }
            }
            if(!right_have_found){
                rright->valid_row.push_back(tmp_y);
                rright->boundary[tmp_y] = COL -1;
                right_have_found = 1;
            }
        }
        else{
            for(int x = last_x+1; x < COL; x++){
                if((img[tmp_y][x] == 255)&& img[tmp_y][x-1] == 0){
                    rright->boundary[tmp_y] = x;
                    right_have_found =1;
                    break;
                }
            }
        }
    } 
    else{// 上一行右边界为159，本行使用传统方式寻找跳变点
        rightFindChange(rright,tmp_y,last_mid);
        right_have_found = 1;
    }
    if(!right_have_found){
        rright->boundary[tmp_y] = COL-1;
        rright->loss.push_back(tmp_y);
    }
    return rright->boundary[tmp_y];

}


/**
 * @description: 八领域生成左右边界的
 * @param {Direction*} pleft
 * @param {Direction*} pright
 * @param {int16} y_min
 * @param {int16} y_max
 * @return {*}
 */
void filedGetLRChangePoints(Direction* pleft,Direction* pright, int16 y_min=0,int16 y_max=60){
    int16 curr_right = 0;
    int16 curr_left = 0;
    int16 curr_mid = 0;
    // 使用传统寻找跳变点方式得到第一行的左右边界及中线
    curr_right = rightFindChange(pright,ROW-1,MIDDLE);
    curr_left = leftFindChange(pleft, ROW-1, MIDDLE);
    curr_mid = (int16)(curr_right+curr_left)/2;
    midLine[ROW-1] = curr_mid;
    // 对每一行进行寻找左右边界点
    for(int y = ROW-2;y > y_min-1; y--){
        curr_right = filedRightFindChange(pright,y,curr_right,curr_mid);
        curr_left = filedLeftFindChange(pleft,y,curr_left,curr_mid);
        midLine[y] = (int16)(curr_right+curr_left)/2;
        curr_mid = midLine[y];

        // 异常行检测
        if(!V_turn_flag && (curr_left - curr_right > 25) && y < 70){
            abnormal_line = y;
            V_turn_flag = true;
        }
    }

}

/**
 * @description: 寻找最远有效行
 * @param {Direction*} pleft 左边的结构体指针
 * @param {Direction*} pright 右边的结构体指针
 * @return {*}
 */
void findValidRow(Direction* pleft,Direction* pright){
    // 获得可能的有效行的vector长度
    int16 L_len = pleft->valid_row.size();
    int16 R_len = pright->valid_row.size();
    // 选取最靠近图像底部的有效行
    if((L_len==0) && (R_len>0)) 
        validRow = pright->valid_row[0];
    if (L_len >0 && R_len==0)
        validRow = pleft->valid_row[0];
    if(L_len >0 && R_len>0){
         if (pleft->valid_row[0]<pright->valid_row[0])
            validRow = pright->valid_row[0];
        else
            validRow = pleft->valid_row[0];
    }
    // TODO:若两个都是0的情况呢
}

/**
 * @description: 根据两点修复一条直线
 * @param {Direction*} pleft
 * @param {Direction*} pright
 * @param {int16} down_x
 * @param {int16} down_y
 * @param {int16} up_x
 * @param {int16} up_y
 * @param {bool} dir
 * @return {*}
 */
void repairLine(Direction* pleft, Direction* pright, int16 down_x, int16 down_y, int16 up_x, int16 up_y,bool dir){
    if (dir ==LEFT_DIR){
       float k = (float)(up_x -down_x)/(down_y - up_y);
       for(int i = down_y -1; i > up_y; i--){
           pleft->boundary[i] = down_x +(int)(k*(down_y-i));
           // 下面这句只是为了调试的时候方便查看图像
           img[i][pleft->boundary[i]] =100;
       }
    }
    else{
        float k = (float)( down_x - up_x)/(down_y - up_y);
        for(int i = down_y -1; i > up_y; i--){
           pright->boundary[i] = down_x - (int)(k*(down_y-i));
           // 下面这句只是为了调试的时候方便查看图像
           img[i][pright->boundary[i]] =100;
        }
    } 
}

/**
 * @description: 重新计算中线
 * @param {Direction*} pleft
 * @param {Direction*} pright
 * @return {*}
 */
void reCalMidLine(Direction* pleft,Direction* pright){
    for(int i = 0; i<ROW-1; i++){
        midLine[i] = (int)((pleft->boundary[i]+pright->boundary[i])/2);
    }
}

/**
 * @description: 八领域中寻找白点的个数
 * @param {int} i
 * @param {int} j
 * @return {*}
 */
int16 findNineSquares(int i, int j){
    int16 count = 0;
    for (int k = 0; k < 3; k++){
        for(int l = 0; l < 3; l++){
            if (img[i-1+k][j-1+l]==255){
                count++;
            }
        }
    }
    return count;
    
}


/**
 * @description: 获取可能的十字拐点，对于左边，符合上一行为0，
 *               下一行不为0（或相反）的就视为可能的十字拐点，
 *               对于右边，符合上一行为159，
 *               下一行不为159（或相反）视为可能的十字拐点
 * @param {Direction*} pleft
 * @param {Direction*} pright
 * @param {int16} y_min
 * @return {*}
 */
void getPossibleCrossPoint(Direction* pleft,Direction* pright,int16 y_min = 40){
    for(int i = ROW -1; i > y_min ;i--){
        // 左边可能的上拐点
        if((pleft->boundary[i] == 0)&&(pleft->boundary[i-1] != 0)){
            pleft->possible_up_cross_point.push_back(i-1);
        }
        // 左边可能的下拐点
        if((pleft->boundary[i] != 0)&&(pleft->boundary[i-1] == 0)){
            pleft->possible_down_cross_point.push_back(i);
        }
        // 右边可能的上拐点
        if((pright->boundary[i] == COL-1)&&(pright->boundary[i-1] != COL-1)){
            pright->possible_up_cross_point.push_back(i-1);
        }
        // 右边可能的下拐点
        if((pright->boundary[i] != COL-1)&&(pright->boundary[i-1] == COL-1)){
            pright->possible_down_cross_point.push_back(i);
        }
    }
}


/**
 * @description: 十字识别
 * @param {int} y_min
 * @param {Direction*} pleft
 * @param {Direction*} pright
 * @return {*}
 */
void crossReg(int y_min,Direction* pleft, Direction* pright){
    bool R_down_flag =0;   
    bool R_up_flag = 0;
    bool L_down_flag = 0;
    bool L_up_flag = 0;
    getPossibleCrossPoint(pleft,pright,0);
    // 先找右边
    // 右下拐点
    int r_down_count = pright->possible_down_cross_point.size();
    // 倒序遍历，方便pop掉不符合的拐点
    for(int item = r_down_count-1 ;item >= 0;item --){
        int16 tmp_y = pright->possible_down_cross_point[item];
        if((tmp_y>=5) && (tmp_y<= ROW -5)&&(tmp_y > validRow)){
            for (int i = tmp_y; i <= tmp_y + 4; i++){
                if(pright->boundary[i]<=pright->boundary[i+1] 
                && pright->boundary[i+1] <= pright->boundary[i+2]
                && myabs(pright->boundary[i],pright->boundary[i+2])<5
                && pright->boundary[i]<COL-10){
                    pright->possible_down_cross_point[item]=i;
                    break;
                }
                if(i == tmp_y+4){
                    //删除掉不符合的
                    pright->possible_down_cross_point.erase(pright->possible_down_cross_point.begin()+item);
                }
            }
        }
        else pright->possible_down_cross_point.erase(pright->possible_down_cross_point.begin()+item);
        //tmp_y >COL-5的不处理  
    }
    //右上拐点
    int16 r_up_count = pright->possible_up_cross_point.size();
    for(int item = r_up_count-1 ;item >= 0;item --){
        int16 tmp_y = pright->possible_up_cross_point[item];
        if((tmp_y>=5) && (tmp_y<= ROW -5)&&(tmp_y > validRow)){
            for (int i = tmp_y; i >= tmp_y - 4; i--){
                if(pright->boundary[i]>=pright->boundary[i-1] 
                && pright->boundary[i-1] >= pright->boundary[i-2]
                && myabs(pright->boundary[i],pright->boundary[i-2])<5
                && pright->boundary[i] < COL-10){
                    pright->possible_up_cross_point[item]=i;
                    break;
                }
                if(i == tmp_y-4){
                    //删除掉不符合的
                    pright->possible_up_cross_point.erase(pright->possible_up_cross_point.begin()+item);
                }
            }
        }
        else pright->possible_up_cross_point.erase(pright->possible_up_cross_point.begin()+item);
        //tmp_y >COL-5的不处理  
    }
    // 左下拐点
    int16 l_dowm_count  = pleft->possible_down_cross_point.size();
    for(int item = l_dowm_count-1 ;item >= 0;item --){
        int16 tmp_y = pleft->possible_down_cross_point[item];
        if((tmp_y>=5) && (tmp_y<= ROW -5)&&(tmp_y > validRow)){
            for (int i = tmp_y; i <= tmp_y + 4; i++){
                if(pleft->boundary[i]>=pleft->boundary[i+1] 
                && pleft->boundary[i+1] >= pleft->boundary[i+2]
                && myabs(pleft->boundary[i],pleft->boundary[i+2])<5
                && pleft->boundary[i]>10){
                    pleft->possible_down_cross_point[item]=i;
                    break;
                }
                if(i == tmp_y+4){
                    //删除掉不符合的
                    pleft->possible_down_cross_point.erase(pleft->possible_down_cross_point.begin()+item);
                }
            }
        }
        else pleft->possible_down_cross_point.erase(pleft->possible_down_cross_point.begin()+item);
        //tmp_y >COL-5的不处理  
    }
    //左上拐点
    int16 l_up_count  = pleft->possible_up_cross_point.size();
    for(int item = l_up_count-1 ;item >= 0;item --){
        int16 tmp_y = pleft->possible_up_cross_point[item];
        if((tmp_y>=5) && (tmp_y<= ROW -5)&&(tmp_y > validRow)){
            for (int i = tmp_y; i >= tmp_y - 4; i--){
                if((pleft->boundary[i]<=pleft->boundary[i-1])
                && (pleft->boundary[i-1] <= pleft->boundary[i-2])
                &&  (myabs(pleft->boundary[i],pleft->boundary[i-2])<5)
                && (pleft->boundary[i]>10)
                && (i>10)){
                    pleft->possible_up_cross_point[item]=i;
                    break;
                }
                if(i == tmp_y-4){
                    //删除掉不符合的
                    pleft->possible_up_cross_point.erase(pleft->possible_up_cross_point.begin()+item);
                }
            }
        }
        else pleft->possible_up_cross_point.erase(pleft->possible_up_cross_point.begin()+item);
        //tmp_y >COL-5的不处理  
    }
    // 为了刚好地显示
    #if(DEBUG)
        for (int i = 0; i < pleft->possible_down_cross_point.size(); i++)
        {
            int16 tmp_y = pleft->possible_down_cross_point[i];
            img[tmp_y][pleft->boundary[tmp_y]] = 100;
        }
        for (int i = 0; i < pleft->possible_up_cross_point.size(); i++)
        {
            int16 tmp_y = pleft->possible_up_cross_point[i];
            img[tmp_y][pleft->boundary[tmp_y]] = 100;
        }
        for (int i = 0; i < pright->possible_down_cross_point.size(); i++)
        {
            int16 tmp_y = pright->possible_down_cross_point[i];
            img[tmp_y][pright->boundary[tmp_y]] = 100;
        }
        for (int i = 0; i < pright->possible_up_cross_point.size(); i++)
        {
            int16 tmp_y = pright->possible_up_cross_point[i];
            img[tmp_y][pright->boundary[tmp_y]] = 100;
        }
    #endif

}


/**
 * @description: 经过十字的识别后，对十字的类型进行判断并修复边线
 * @param {Direction*} pleft
 * @param {Direction*} pright
 * @return {*}
 */
void regCrossType(Direction* pleft,Direction* pright){
    int16 l_up_count = pleft->possible_up_cross_point.size();
    int16 l_down_count = pleft->possible_down_cross_point.size();
    int16 r_up_count = pright->possible_up_cross_point.size();
    int16 r_down_count = pright->possible_down_cross_point.size();
    Point L_UP,L_DOWN,R_UP,R_DOWN;
    // 判断4个拐点的vector长度是否为0
    if(l_up_count!=0){
        L_UP.y = pleft->possible_up_cross_point.back();
        L_UP.x = pleft->boundary[L_UP.y];
    }
    if(l_down_count!=0){
        L_DOWN.y = pleft->possible_down_cross_point[0];
        L_DOWN.x = pleft->boundary[L_DOWN.y];
    }
    if(r_up_count!=0){
        R_UP.y = pright->possible_up_cross_point.back();
        R_UP.x = pright->boundary[R_UP.y];
    }
    if(r_down_count!=0){
        R_DOWN.y = pright->possible_down_cross_point[0];
        R_DOWN.x = pright->boundary[R_DOWN.y];
    }
    // 修复左边
    if(l_down_count && l_up_count){
        if(L_DOWN.y > L_UP.y){
            repairLine(pleft,pright,L_DOWN.x,L_DOWN.y,L_UP.x,L_UP.y,LEFT_DIR);
            cross_flag = true;
        }
        else{
            // 往图像左上角处修复
            repairLine(pleft,pright,L_DOWN.x,L_DOWN.y,L_DOWN.x+10,0-1,LEFT_DIR);
            // 往图像左下角处修复
            repairLine(pleft,pright,0,ROW-1,L_UP.x,L_UP.y,LEFT_DIR);
            cross_flag = true;
        }
        
    }
    // 左边只有下拐点
    else if(l_down_count && (l_up_count==0)){
        // 往图像左上角处修复
        repairLine(pleft,pright,L_DOWN.x,L_DOWN.y,L_DOWN.x +10,0-1,LEFT_DIR);
        cross_flag = true;
    }
    // 左边只有上拐点
    else if(l_up_count && (l_down_count==0)){
        // 往图像左下角处修复
        repairLine(pleft,pright,0,ROW-1,L_UP.x,L_UP.y,LEFT_DIR);
        cross_flag = true;
    }
    else{
       cross_flag = false;
    }
    // 修复右边
    if(r_down_count && r_up_count ){
        if (R_DOWN.y > R_UP.y){
            // 2个拐点之间进行修复
            repairLine(pleft,pright,R_DOWN.x,R_DOWN.y,R_UP.x,R_UP.y,RIGHT_DIR);
            cross_flag = true;
        }
        else{
            // 往图像右上角处修复
            repairLine(pleft,pright,R_DOWN.x,R_DOWN.y,R_DOWN.x-10,0-1,RIGHT_DIR);
            // 往图像右下角处修复
            repairLine(pleft,pright,COL-1,ROW-1,R_UP.x,R_UP.y,RIGHT_DIR);
            cross_flag = true;
        }   
    }
    else if(r_down_count && (r_up_count==0)){
        // 往图像右上角处修复
        repairLine(pleft,pright,R_DOWN.x,R_DOWN.y,R_DOWN.x - 10,0-1,RIGHT_DIR);
        cross_flag = true;
    }
    else if(r_up_count && (r_down_count==0) && (R_UP.y >20)){
        // 往图像右下角处修复
        repairLine(pleft,pright,COL-1,ROW-1,R_UP.x,R_UP.y,RIGHT_DIR);
        cross_flag = true;
    }
    // 如果左边也是cross_flag=false，右边才false
    else if(cross_flag == false){
        cross_flag = false;
    }

    //特殊情况
    if(r_down_count && l_up_count==0 && l_down_count==0){
        cross_flag =false;
    }
    if(cross_flag){
        reCalMidLine(pleft,pright);
    }
}


/**
 * @description: 弯道识别，如果有十字就没有弯道识别了，
 *               主要根据loss来
 * @param {Direction*} pleft
 * @param {Direction*} pright
 * @return {*}
 */
void turnReg(Direction* pleft,Direction* pright){
    if(cross_flag)turn_flag = false;
    else{
        int16 left_loss_count = pleft->loss.size();
        int16 right_loss_count = pright->loss.size();
        // 右弯道
        if(left_loss_count < 50 && right_loss_count >60 && *max_element(&pleft->boundary[0],&pleft->boundary[ROW-1])>40){
            // 平移补线法
            for (int i = ROW-1; i > -1; i--){
                midLine[i] = pleft->boundary[i] + HALF_ROAD_WIDTH;
            }  
            turn_flag  = true;
        }
        else if(right_loss_count < 50 && left_loss_count >60 && *min_element(&pright->boundary[0],&pright->boundary[ROW-1])<120){
            // 平移补线法
            for (int i = ROW-1; i > -1; i--) {
                midLine[i] = pright->boundary[i] - HALF_ROAD_WIDTH;
            }
            turn_flag  = true;
        }
        if(!turn_flag){
            // 在转弯/V型弯出会出现validRow>0的情况，此为左转弯/左V型弯
            if(validRow > 0 && validRow < 25 && (pright->boundary[validRow] == COL - 1)){
                // 平移补线法
                for (int i = ROW-1; i > -1; i--){
                    midLine[i] = pright->boundary[i] - HALF_ROAD_WIDTH;
                }  
                turn_flag  = true;
            }
            // 此为右转弯/右V型弯
            else if(validRow > 0 && validRow < 25 && (pleft->boundary[validRow] == 0)){
                // 平移补线法
                for (int i = ROW-1; i > -1; i--) {
                    midLine[i] = pleft->boundary[i] + HALF_ROAD_WIDTH;
                }
                turn_flag  = true;
            }
        }
    }
}


void V_turnReg(Direction* pleft,Direction* pright){
    if(!cross_flag && !turn_flag && V_turn_flag){
        if(pright->boundary[abnormal_line] <= 30){
            // 左V型弯
            // 平移补线法
            for (int i = ROW-1; i > -1; i--){
                midLine[i] = pright->boundary[i] - HALF_ROAD_WIDTH;
            }  
        }
        else if(pright->boundary[abnormal_line] >= 130){
            // 右V型弯
            // 平移补线法
            for (int i = ROW-1; i > -1; i--){
                midLine[i] = pleft->boundary[i] + HALF_ROAD_WIDTH;
            }  
        }
    }
}

/**
 * @description: 调试时显示边界
 * @param {Direction*} pleft
 * @param {Direction*} pright
 * @return {*}
 */
void showBoundary(Direction* pleft,Direction* pright){
    for (int i = 0; i < ROW; i++){
        img[i][pleft->boundary[i]] = 20;
    }
    for (int i = 0; i < ROW; i++){
        img[i][pright->boundary[i]] = 20;
    }
    
    for (int i = 0; i < ROW; i++){
        if (midLine[i] <=0){
            midLine[i] = 0;
        }
        img[i][midLine[i]] = 20;
    }   
}


/**
 * @description: 显示数组到文件中
 * @param {int16} img
 * @return {*}
 */
void showMat(IMG_TYPE img[][COL],Direction* pleft, Direction* pright){
    showBoundary(pleft, pright);
    Direction* lldj = new Direction;
    ofstream outfile;
    outfile.open("output.txt");
    for (int i = 0; i < ROW; i++){
		for (int j = 0; j < COL; j++){
            if(img[i][j]==255){
                outfile<<"*";
            }
            else if(img[i][j]==100){
                outfile<<"@";
            }
            else if(img[i][j]==20){
                outfile<<"$";
            }
            else{
                outfile<<".";
            }	
		}
        outfile<<endl;
	}
    outfile.close();

}

/**
 * @description: 获得文件夹下的所有文件名
 * @param {*}
 * @return {*}
 */
auto getFileNames()
{
    vector<string>filelist;
    string tmp_name;
    ifstream infilename;//定义读取文件流，相对于程序来说是in
	infilename.open("filename.txt");//打开文件
    for(int j =0;j<2382;j++){
        infilename>>tmp_name;
        filelist.push_back(tmp_name);
        // cout<<tmp_name<<endl;
    }
    return filelist;
}


int calMidError(){
    int sum = 0;
    if(validRow <= 60){
        for(int i = 60; i<ROW; i++){
            sum += midLine[i];
        }
        return (int)((sum-MIDDLE*60)/60);
    }
    else{
        for(int i = (int)validRow; i<ROW; i++){
            sum +=midLine[i];
        }
        return (int)((sum-MIDDLE*(120-validRow))/(120-validRow));
    }
    
}

int cruise(uchar* matrix){
    clock_t startTime,endTime;
    startTime = clock();//计时开始
    getMat(matrix);
    Direction* struct_left = new Direction;
    Direction* struct_right = new Direction;
    filedGetLRChangePoints(struct_left,struct_right,0);
    findValidRow(struct_left,struct_right);
    crossReg(0,struct_left,struct_right);
    regCrossType(struct_left,struct_right);
    turnReg(struct_left,struct_right);
    V_turnReg(struct_left,struct_right);
    showMat(img,struct_left,struct_right);
    // cout<<"turn_flag:"<<turn_flag<<endl;
    // cout<<"cross_flag:"<<cross_flag<<endl;
    // cout<<"validRow:"<<validRow<<endl;
    // cout<<"V_turn_flag:"<<V_turn_flag<<endl;
    int mid_error = calMidError();
    cout<<"error:"<<mid_error<<endl;
    delete struct_left,struct_right;
    turn_flag = false;
    cross_flag = false;
    V_turn_flag = false;
    abnormal_line = 0;
    validRow = 0;
    endTime = clock();//计时结束
    // cout<<"cpp time:"<<(double)(endTime - startTime) /1000<<endl;
    return mid_error;
}


int main(){
    clock_t startTime,endTime;
    
    auto namelist = getFileNames();
     
    for(string name:namelist){
        cout<<name<<endl;
        if(name =="imgTxt/149.jpg.txt"){
            cout<<endl;
        }
        creatMat(img,name);
        startTime = clock();//计时开始
        Direction* struct_left = new Direction;
        Direction* struct_right = new Direction;
        filedGetLRChangePoints(struct_left,struct_right,0);
        // for(int16 item:struct_right->valid_row){
        //     cout<<"loss:"<<item<<endl;
        // }
        findValidRow(struct_left,struct_right);
        crossReg(0,struct_left,struct_right);
        regCrossType(struct_left,struct_right);
        turnReg(struct_left,struct_right);
        V_turnReg(struct_left,struct_right);
        int mid_error = calMidError();

        showMat(img,struct_left,struct_right);
        #if(DEBUG)
            cout<<"turn_flag:"<<turn_flag<<endl;
            cout<<"cross_flag:"<<cross_flag<<endl;
            cout<<"V_turn_flag:"<<V_turn_flag<<endl;
            cout<<"validRow:"<<validRow<<endl;
        #endif
        cout<<"mid_error:"<<mid_error<<endl;
        delete struct_left,struct_right;
        turn_flag = false;
        cross_flag = false;
        V_turn_flag = false;
        abnormal_line = 0;
        validRow = 0;
        endTime = clock();//计时结束
        cout<<"t:"<<(double)(endTime - startTime) /1000<<endl;
        getchar();
    }  
    
    return 0 ;
}


// LINUX编译：g++ helloworld.cpp -o helloworld
// 运行：./helloworld
// 编译成动态库：g++ -o cruise.so -shared -fPIC cruise.cpp